/*
 * Show Line of CARLA 0.9.1 Town1
 */
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
 
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
 
#include <cmath>
 
using namespace std;
 
int main(int argc, char** argv)
{
    vector<double> location_x, location_y, location_z;
    /* Read data from point_right.csv */
    ifstream fin("Topology.csv");
    string line;
    while (getline(fin, line)) 
    {
        istringstream sin(line);
        vector<string> Waypoints;
        string info;
        while (getline(sin, info, ',')) {
            Waypoints.push_back(info);
        }
        // Get x,y,z of points and transform to double
        string x_str = Waypoints[3];
        string y_str = Waypoints[4];
        string z_str = Waypoints[5];
        double x, y, z;
        stringstream sx, sy, sz;
        sx << x_str;
        sy << y_str;
        sz << z_str;
        sx >> x;
        sy >> y;
        sz >> z;
        //cout << "x= " << x << "  " << "y= " << y << "  " << "z= " << z << endl;
        cout << "Read data done!" << endl;
 
        location_x.push_back(x);
        location_y.push_back(y);
        location_z.push_back(z);
    }
 
    /* Define Marker Line */
    ros::init(argc, argv, "MapLine");
    ros::NodeHandle n;
    ros::Publisher markerArray_pub = n.advertise<visualization_msgs::MarkerArray>("MarkerArray", 10);
    ros::Rate r(1);
 
    
    while (ros::ok())
    {
        visualization_msgs::MarkerArray lines, MapPoints;
        for (int i = 1; i < location_x.size(); i=i+2)
        {
            // Create lines and points
            visualization_msgs::Marker line_strip, points;
            line_strip.header.frame_id = points.header.frame_id = "/my_frame";
            line_strip.header.stamp = points.header.stamp = ros::Time::now();
            line_strip.ns = points.ns = "points_and_lines";
            line_strip.action = points.action = visualization_msgs::Marker::ADD;
            //line_strip.pose.orientation.w = 1.0;
            line_strip.id = i;
            line_strip.type = visualization_msgs::Marker::LINE_STRIP;
            line_strip.scale.x = 0.1;
            line_strip.color.g = 1.0;
            line_strip.color.a = 1.0;
 
            points.id = 0;
            points.type = visualization_msgs::Marker::POINTS;
            points.scale.x = 2;
            points.scale.y = 2;
 
            points.color.r = 1.0f;
            points.color.a = 1.0;
 
            // line_strip obtains points
            geometry_msgs::Point p, q;
            p.x = location_x[i];
            p.y = location_y[i];
            p.z = location_z[i];
            q.x = location_x[i+1];
            q.y = location_y[i+1];
            q.z = location_z[i+1];
            cout << i << "x= " << location_x[i] << "   " << "y= " << location_y[i] << endl; 
            cout << i+1 << "x= " << location_x[i+1] << "   " << "y= " << location_y[i+1] << endl;
            cout << endl << endl;
 
            line_strip.points.push_back(p);        
            line_strip.points.push_back(q);
            points.points.push_back(p);
            points.points.push_back(q);
 
            lines.markers.push_back(line_strip);
            MapPoints.markers.push_back(points);         
        }
 
        cout << "MarkerArray lines size: " << lines.markers.size() << endl;
        markerArray_pub.publish(lines);
        markerArray_pub.publish(MapPoints);
        r.sleep();
    }
 
    cout << "Right Line Done!";
    return 0;
}
